﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.Drawing;

namespace Nate.Pathfinding
{
    public class AStar : Pathfinding.IPathfind
    {
        // 
        // Public Methods 
        //

        public void Init(NodeID start_id, NodeID dest_id, Graph graph)
        {
            _success = false;
            _graph = graph;
            _source = start_id.GraphIndex;
            _destination = dest_id.GraphIndex;
            _destination_node = _graph.Nodes[_destination];

            _costs_g = NUtils.Array.AllocateWithDefault<double>(_graph.Nodes.Count, 0);
            _costs_f = NUtils.Array.AllocateWithDefault<double>(_graph.Nodes.Count, 0);

            Node source_node = _graph.Nodes[_source];
            _costs_f[_source] = _Heuristic(source_node);

            _shortest_path_tree = new Edge[_graph.Nodes.Count];
            _search_frontier = new Edge[_graph.Nodes.Count];

            _queue = new NUtils.PriorityQueue<Node>(_graph.Nodes.Count, _CompareNodes);
            _queue.Insert(_graph.Nodes[_source]);
            
        }

        public AStar()
        {
            _options = new Nate.Shared.Utils.Options();
            _options.Label = "Heuristics";
            _options.ToolTipText = "Available Heuristics";
            _options.Selections = new string[] { "Distance^2", /*"Distance",*/ "Manhattan" };
            _heuristic = Heuristics.DistanceSquared;
        }

        public Shared.Status Tick()
        {
            if (_success)
            {
                return Shared.Status.Success;
            }

            if (_queue.Empty)
            {
                return Shared.Status.Failure;
            }


            return _tick();
        }

        // 
        // Public Properties 
        //

        public List<Point> Path
        {
            get
            {
                List<Point> points = new List<Point>();

                if (_success)
                {
                    int i = _destination;
                    while (i != _source)
                    {
                        points.Add(_graph.Nodes[i].Point);

                        Edge e = _shortest_path_tree[i];
                        if (null == e)
                        {
                            break;
                        }

                        i = e.Other(i);
                    }

                    points.Reverse();
                }
                else
                {
                    // Let the caller see the nodes that we've visited.

                    foreach (Edge e in _shortest_path_tree)
                    {
                        if (null == e)
                        {
                            continue;
                        }

                        points.Add(_graph.Nodes[e.A].Point);
                        points.Add(_graph.Nodes[e.B].Point);
                    }
                }

                return points;
            }
        }

        public Nate.Shared.Utils.Options Options
        {
            get
            {
                return _options;
            }
        }

        public string OptionSelection
        {
            set
            {
                if (value == Heuristics.Distance.ToString())
                {
                    _heuristic = Heuristics.Distance;
                }
                else if (value == Heuristics.Manhattan.ToString())
                {
                    _heuristic = Heuristics.Manhattan;
                }
                else
                {
                    _heuristic = Heuristics.DistanceSquared;
                }
            }
        }

        // 
        // Private Objects
        //

        enum Heuristics
        {
            DistanceSquared,
            Distance,
            Manhattan
        }

        // 
        // Private Data
        //

        private Heuristics _heuristic;
        private Shared.Utils.Options _options;
        private bool _success;
        private int _source;
        private int _destination;
        private Node _destination_node;
        private Graph _graph;
        private double[] _costs_g;
        private double[] _costs_f;
        private Edge[] _shortest_path_tree;
        private Edge[] _search_frontier;
        private NUtils.PriorityQueue<Node> _queue;

        //
        // Private Methods
        //

        private Shared.Status _tick()
        {
            Node next_best = _queue.Dequeue();
            int next_index = next_best.Index;

            _shortest_path_tree[next_index] = _search_frontier[next_index];

            if (_destination == next_index)
            {
                _success = true;
                return Shared.Status.Success;
            }

            foreach (Edge e in next_best.Edges)
            {
                int neighbor_node = e.Other(next_index);
                double cost_h = _Heuristic(_graph.Nodes[neighbor_node]);
                double cost_g = _costs_g[next_index] + e.Cost;

                if (null == _search_frontier[neighbor_node])
                {
                    _costs_f[neighbor_node] = cost_g + cost_h;
                    _costs_g[neighbor_node] = cost_g;
                    _search_frontier[neighbor_node] = e;
                    _queue.Insert(_graph.Nodes[neighbor_node]);
                }
                else if (cost_g < _costs_g[neighbor_node] &&
                         null == _shortest_path_tree[neighbor_node])
                {
                    _costs_f[neighbor_node] = cost_g + cost_h;
                    _costs_g[neighbor_node] = cost_g;
                    _search_frontier[neighbor_node] = e;
                    _queue.PriorityChanged();
                }
            }

            return Shared.Status.InProcess;
        }

        private int _CompareNodes(Node a, Node b)
        {
            double cost_a = _costs_f[a.Index];
            double cost_b = _costs_f[b.Index];
            int comp = cost_a.CompareTo(cost_b);
            return comp;
        }

        private double _Heuristic(Node n)
        {
            switch (_heuristic)
            {
                case Heuristics.DistanceSquared:
                    return _destination_node.DistanceSquared(n);
                case Heuristics.Manhattan:
                    return _destination_node.DistanceManhattan(n);
                case Heuristics.Distance:
                    return _destination_node.Distance(n);
            }

            return _destination_node.DistanceSquared(n);
        }
    }
}
